import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
import yaml
import sys
import os

def load_waypoints(yaml_file):
    """读取YAML文件并转换为PoseStamped列表"""
    if not os.path.exists(yaml_file):
        print(f"错误: 找不到文件 {yaml_file}")
        return []

    with open(yaml_file, 'r') as f:
        data = yaml.safe_load(f)
    
    # 兼容之前的脚本格式，可能是 {'waypoints': [...]} 或者是直接的列表
    wp_list = data.get('waypoints', []) if isinstance(data, dict) else data

    poses = []
    for point in wp_list:
        pose = PoseStamped()
        pose.header.frame_id = 'map'  # 必须指定坐标系
        pose.header.stamp = rclpy.time.Time().to_msg() # 加上时间戳
        
        pose.pose.position.x = float(point['x'])
        pose.pose.position.y = float(point['y'])
        # z 通常是 0
        pose.pose.position.z = 0.0 
        
        # 读取四元数朝向
        pose.pose.orientation.z = float(point['z'])
        pose.pose.orientation.w = float(point['w'])
        # x, y 通常是 0
        pose.pose.orientation.x = 0.0
        pose.pose.orientation.y = 0.0
        
        poses.append(pose)
    
    return poses

def main():
    rclpy.init()
    
    # 1. 初始化 Nav2 API
    navigator = BasicNavigator()
    
    # 文件名 (如果你生成的文件名不一样，请修改这里)
    yaml_filename = '/home/jingzedong/B2_ws/src/b2_nav_bringup/params/tunnel_final_path.yaml' 
    
    print(f"正在加载巡检点: {yaml_filename} ...")
    inspection_poses = load_waypoints(yaml_filename)
    
    if not inspection_poses:
        print("未加载到有效点，程序退出。")
        return

    # 2. 等待 Nav2 启动
    print("等待 Nav2 激活...")
    # navigator.waitUntilNav2Active() # 如果是仿真环境建议开启，实车有时需要注释掉这行手动确认
    
    # 3. 发送巡检指令
    print(f"开始执行巡检任务！共有 {len(inspection_poses)} 个点。")
    # 这里的关键是 followWaypoints
    navigator.followWaypoints(inspection_poses)

    # 4. 监控任务进度
    i = 0
    while not navigator.isTaskComplete():
        i += 1
        feedback = navigator.getFeedback()
        if feedback and i % 5 == 0: # 每5个循环打印一次，防止刷屏
            print(f'[执行中] 正在前往第 {feedback.current_waypoint + 1} 个点...')
            
            # 你可以在这里加入简单的逻辑，比如：
            # if feedback.current_waypoint == 5:
            #     print("即将到达关键区域！")

    # 5. 处理结果
    result = navigator.getResult()
    if result == TaskResult.SUCCEEDED:
        print('✅ 巡检任务全部完成！')
    elif result == TaskResult.CANCELED:
        print('⚠️ 任务被取消')
    elif result == TaskResult.FAILED:
        print('❌ 任务失败')

    navigator.lifecycleShutdown()
    sys.exit(0)

if __name__ == '__main__':
    main()
